#include "motor.h"

/**
 * Sets the motor speed - values allowed are in between
 * MOTOR_FULL_BACKWARDS and MOTOR_FULL_FORWARDS.
 */
void set_motor_speed(byte spd) {
	if (old_spd > spd + 5) {		
		spd = brake(old_spd, spd);
		old_spd = spd;
	}else{
		old_spd = spd;
	}
	
	if (spd < MOTOR_FULL_BACKWARDS) {
		spd = MOTOR_FULL_BACKWARDS;
	} else if (spd > MOTOR_FULL_FORWARDS) {
		spd = MOTOR_FULL_FORWARDS;
	}
	if (spd < MOTOR_NULL && spd > MOTOR_MIN_BACKWARDS) {
		spd = MOTOR_MIN_BACKWARDS;
	} else if (spd > MOTOR_NULL && spd < MOTOR_MIN_FORWARDS) {
		spd = MOTOR_MIN_FORWARDS;
	}
	
	set_servo(1, spd);
	//set_servo(1, MOTOR_NULL);
	return;
}

void stop_motor(){
	set_motor_speed(MOTOR_NULL);
}

byte brake(byte old, byte new) {
	byte diff = old - new;
	return (MOTOR_MIN_BACKWARDS - 5*diff);
}
